#include "Tools.hpp"

void ClarkeTransformation(ClarkeParame_P Clarke)
{
    if (Clarke->Ic)
    {
        float mid = (1.f / 3) * (Clarke->Ia + Clarke->Ib + Clarke->Ic);
        Clarke->Ia -= mid;
        Clarke->Ib -= mid;
    }
    Clarke->I_alpha = Clarke->Ia;
    Clarke->I_beta = Clarke->Ia * _SQRT3_3 + Clarke->Ib * _SQRT3_2_3;
}

void ParkTransformation(ParkParame_P Park)
{
    Park->Id = Park->I_alpha * Park->Angle_Cos + Park->I_beta * Park->Angle_Sin;
    Park->Iq = Park->I_beta * Park->Angle_Cos - Park->I_alpha * Park->Angle_Sin;
}

float LowPassFilter::operator()(float _input)
{
    float time = Time::getTime();
    float dt = time - timeStamp;

    if (dt < 0.0f) dt = 1e-3f;
    else if (dt > 0.3f)
    {
        outputLast = _input;
        timeStamp = time;
        return _input;
    }

    float alpha = timeConstant / (timeConstant + dt);
    float output = alpha * outputLast + (1.0f - alpha) * _input;
    outputLast = output;
    timeStamp = time;

    return output;
}

uint16_t LowPassFilter::operator()(uint16_t _input)
{
    float time = Time::getTime();
    float dt = time - timeStamp;

    if (dt < 0.0f) dt = 1e-3f;
    else if (dt > 0.3f)
    {
        outputLast = _input;
        timeStamp = time;
        return _input;
    }

    float alpha = timeConstant / (timeConstant + dt);
    uint16_t output = alpha * outputLast + (1.0f - alpha) * _input;
    outputLast = output;
    timeStamp = time;

    return output;
}